﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using CrystalCore.Mapping;
using Microsoft.Xna.Framework;

namespace CrystalCore.AI
{
    public struct Waypoint
    { 
        //Methods
		private Vector3 _WaypointPosition;
        public Vector3 WaypointPosition
		{
			get { return _WaypointPosition; }
			set { _WaypointPosition = value; }
		}

        //cTor
        public Waypoint(Vector3 position)
        {
            _WaypointPosition = position;
        }
        public Waypoint(MazePosition position)
        {
            _WaypointPosition = position.CenterCell;
        }

        public Vector3 DistanceTo(Waypoint b)
        {
            return GetDistanceBetween(this, b);
        }
        public double AngleTo(Waypoint b)
        {
            return GetAngleBetween(this, b);
        }

        //Overloads
        public static Waypoint operator -(Waypoint a, Waypoint b)
        {
            return new Waypoint(a._WaypointPosition - b._WaypointPosition);
        }
        public static implicit operator Vector3(Waypoint a)
        {
            return a._WaypointPosition;
        }
        public static implicit operator Waypoint(Vector3 a)
        {
            return new Waypoint(a);
        } 

        //Statics
        public static double GetAngleBetween(Waypoint a, Waypoint b)
        {
            return Math.Atan2(b._WaypointPosition.Y - a._WaypointPosition.Y, b._WaypointPosition.X - a._WaypointPosition.X);
        }
        public static Vector3 GetDistanceBetween(Waypoint a, Waypoint b)
        {
            Vector3 result = b - a;
            result.X = (float)Math.Round(result.X, 2);
            result.Y = (float)Math.Round(result.Y, 2);
            result.Z = (float)Math.Round(result.Z, 2);

            return result;
        }
    }
}
